/**
* manager the slamcore board ,save & upload maps and robot pose
*
* Created By Yu Jikang @ 2017-5-8
* Copyright (c) 2017 guangdong aoxun
*/
/*

*/
#include <regex>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <time.h>

#include <boost/filesystem.hpp>
#include <iostream> 
#include <fstream> 
#include <algorithm>
#include <iterator>
#include <json/json.h>

#include <rpos/robot_platforms/slamware_core_platform.h>
#include <rpos/robot_platforms/objects/composite_map.h>
#include <rpos/robot_platforms/objects/composite_map_writer.h>
#include <rpos/robot_platforms/objects/composite_map_reader.h>
#include <rpos/features/location_provider/map.h>
#include <rpos/core/pose.h>
#include <rpos/core/action.h>
#include <rpos/features/system_resource.h>
#include <rpos/message/depth_camera_messages.h>
// #include <rpos/features/motion_planner/feature.h>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Empty.h"
#include "std_srvs/Empty.h"
#include "manager/Pose.h"
#include "manager/Switch.h"
#include "manager/Battery.h"

using namespace rpos::core;
using namespace rpos::robot_platforms;
using namespace rpos::features;
using namespace rpos::features::location_provider;
using namespace rpos::system::types;
using namespace rpos::robot_platforms::objects;
using namespace boost;


#define LOCALIZATIONQUALITY 50

#define RANGE (0.5)
static bool isNear(const rpos::core::Pose & home, const rpos::core::Pose & pose, double range=0.5)
{
    double x = pose.x();
    double y = pose.y();

    if ((x < (home.x() + range)) &&
         (x > (home.x() - range)) &&
         (y < (home.y() + range)) &&
         (y > (home.y() - range)))
         return true;
    
    return false;
}

void savePose(rpos::core::Pose & pose) 
{
    Json::FastWriter writer;
    Json::Value jPose;

    jPose["x"] = pose.x();
    jPose["y"] = pose.y();
    jPose["yaw"] = pose.yaw();

    std::string json_str = writer.write(jPose);
    
    boost::filesystem::path cur_path = boost::filesystem::current_path();
    boost::filesystem::path path = cur_path / "pose.json";
    if (boost::filesystem::exists(path)) {
        std::ofstream out(path.filename().c_str());
        if (!out) {
            std::cout << path.filename() << " error1." << std::endl;
            return;
        }
        out << json_str;
    } else {        
        std::ofstream out(path.filename().c_str());
        if (!out) {
            std::cout << path.filename() << " error2." << std::endl;
            return;
        }
        out << json_str;
    }
}
                    // rpos::core::Location location;
                    // rpos::core::Rotation rotation;
bool loadPose(rpos::core::Pose &pose)
{
    Json::Reader reader;
    Json::Value root;
    boost::filesystem::path old_cpath = boost::filesystem::current_path(); //取得当前目录
    boost::filesystem::path file_path = old_cpath / "pose.json"; //path重载了 / 运算符
    if (boost::filesystem::exists(file_path)) {
        std::ifstream in(file_path.filename().c_str());
        if (!in) return false;
        if (!reader.parse(in, root, false)) return false;

        rpos::core::Location loc(root["x"].asDouble(), root["y"].asDouble());
        rpos::core::Rotation rot(root["yaw"].asDouble());
        rpos::core::Pose p(loc, rot);
        pose = p;
        // data = pose;
        return true;
    }
}

SlamwareCorePlatform sdp;
system_resource::HeartBeatToken heartBeatToken;
static bool connected = false;
static std::string ip_addr = "192.168.11.1";
static int port = 1445;
static bool moveMsg = false;
static bool say_disconnected = false;
ros::Publisher tts_pub;
ros::Publisher move_action_pub;
ros::Publisher battery_pub;
static bool say_sth = false;
static int heartbeatInterval = 5;
static rpos::actions::MoveAction moveAction;
static bool home_switch = false;
static int percentage = 100;
static bool g_dc_is_connected = false;
static bool is_backup_map = false;
static bool action_start = false;
static bool action_complete = false;
static time_t action_complete_time = time(NULL);

void doSthWhenActionFinish(const ActionStatus &as, const std::string &target_name)
{
    std_msgs::String result;
    rpos::core::Pose home;
    rpos::core::Pose pose;
    switch (as) {
        case ActionStatus::ActionStatusFinished:
        // result.data = target_name;
        // move_action_pub.publish(result);

        // std_msgs::String result;
        result.data = "到达目的地";
        tts_pub.publish(result);

        break;
        case ActionStatus::ActionStatusError:
        result.data = "无法到达目的地";
        tts_pub.publish(result);
        default:
        break;
    }
    action_complete = true;
    action_complete_time = time(NULL);
}


void cmdCallback(const manager::Pose::ConstPtr& msg)
{
    rpos::core::Location location(msg->x, msg->y);
    if (!connected) {
        std_msgs::String result;
        result.data = "连接已断开,正在重新连接";
        tts_pub.publish(result);
        return;
    }

    if (g_dc_is_connected) {
        std_msgs::String result;
        result.data = "正在充电,无法执行任务";
        tts_pub.publish(result);        
        ROS_INFO("g_dc_is_connected");
        return;
    }

    // moveMsg = true;
    try {
                // rpos::core::Pose home = sdp.getHomePose();
        rpos::core::Pose pose = sdp.getPose();

        rpos::core::Rotation rot(msg->yaw);
        rpos::core::Pose target_pose(location, rot);
        if (isNear(pose, target_pose)) {
            std_msgs::String result;
            result.data = msg->name;
            move_action_pub.publish(result);
            return;
        }

        std_msgs::String result;
        result.data = "好的,跟我走吧";
        tts_pub.publish(result);

        motion_planner::MoveOptions move_opts;
        move_opts.speed_ratio = 200.0;

        moveAction = sdp.moveTo(location, move_opts, 0.0f);
        action_start = true;
        auto actionStatus = moveAction.waitUntilDone();
        std::cout << "actionStatus = " << actionStatus << std::endl;
        std::cout << "actionStatus = " << ActionStatus::ActionStatusError << std::endl;
        // if (actionStatus == ActionStatus::ActionStatusError) {
        //     auto reason = moveAction.getReason();
        //     std::cout << "reason = " << reason << std::endl;
        // }
        doSthWhenActionFinish(actionStatus, msg->name);
    } catch(ConnectionTimeOutException& e) {
        std::cout <<e.what() << std::endl;       
    } catch(ConnectionFailException& e) {
        std::cout <<e.what() << std::endl;
        connected = false;
    }
	ROS_INFO("I heard: [%s]", msg->name.c_str());
}

void timerSaveCallback(const ros::TimerEvent&)
{
    if (connected) {
        try {
            std::string errMsg;
            CompositeMapWriter composite_map_writer;
            CompositeMap composite_map = sdp.getCompositeMap();
            // auto maps = ((CompositeMap)*composite_map.get()).maps();
            auto maps = composite_map.maps();
            // std::cout << "---------" << std::endl;
            // for_each(maps.begin(), maps.end(), [](boost::shared_ptr<MapLayer> _layer){            
            //     std::cout << "name = " << _layer->getName() << std::endl;
            //     std::cout << "type = " << _layer->getType() << std::endl;
            //     std::cout << "usage = " << _layer->getUsage() << std::endl;
            // });
            // std::cout << "---------" << std::endl;
            std::string file_name = is_backup_map ? "map.txt.backup" : "map.txt";
            composite_map_writer.saveFile(errMsg, file_name, composite_map);
            if (!errMsg.empty()) {
                std::cout << errMsg << std::endl;
            }
            is_backup_map = !is_backup_map;
            rpos::core::Pose pose = sdp.getPose();
            savePose(pose);

            //get battery
            percentage = sdp.getBatteryPercentage();
            bool bat_is_charging = sdp.getBatteryIsCharging();
            bool dc_is_connected = sdp.getDCIsConnected();
            manager::Battery battery;
            battery.percentage = percentage;
            battery.dc_in = dc_is_connected;
            battery.charging = bat_is_charging;
            battery_pub.publish(battery);

            if (dc_is_connected && !g_dc_is_connected) {
                std_msgs::String result;
                result.data = "正在充电";
                tts_pub.publish(result);
            }
            if (!dc_is_connected && g_dc_is_connected) {
                std_msgs::String result;
                result.data = "拔掉电源";
                tts_pub.publish(result);
            }
            g_dc_is_connected = dc_is_connected;
        } catch(ConnectionFailException e) {
            std::cout << "save1:" << e.what() << std::endl;                
            connected = false;
        } catch(RequestTimeOutException& e) {
            std::cout << "save1:" << e.what() << std::endl;
        } catch(InvalidArgumentsException& e) {
            std::cout << "save1:" << e.what() << std::endl;
        } catch(OperationFailException& e) {
            std::cout << "timerSaveCallback OperationFailException::" << e.what() << std::endl;
        }
    }
}

void timerHeartBeatCallback(const ros::TimerEvent&)
{
    if (connected) {
        try {
            sdp.refreshHeartBeat(heartBeatToken);
        } catch(OperationFailException& e) {
            std::cout << "timerHeartBeatCallback::" << e.what() << std::endl;
        } catch(InvalidArgumentsException& e) {
            std::cout << "timerHeartBeatCallback:" << e.what() << std::endl;
        }
    }
}

void connectCallback(const ros::TimerEvent&)
{
    if (!connected) {
        std::string msg;
        std_msgs::String result;
        result.data = "导航模块连接已断开";
        tts_pub.publish(result);
        
        try {
            // std::cout << ip->data.c_str() + std::to_string(i) << std::endl;
            // sdp = SlamwareCorePlatform::connect(ip->data.c_str() + std::to_string(i), 1445, 1000);
            sdp = SlamwareCorePlatform::connect(ip_addr, port, 2000);
            if (sdp) {
                connected = true;
                // ip_addr = ip->data.c_str() + std::to_string(i);
                sdp.setMapUpdate(false);

                heartBeatToken = sdp.startHeartBeat(heartbeatInterval);                          
                std::string errMsg;
                CompositeMapReader composite_map_reader;
                boost::shared_ptr<CompositeMap> composite_map = composite_map_reader.loadFile(errMsg, "map.txt");
                
                if (errMsg.empty()) {
                    rpos::core::Pose pose;
                    loadPose(pose);
                    std::cout << pose.x() << "-" << pose.y() << "-" << pose.yaw() << std::endl;
                    sdp.setCompositeMap((CompositeMap)*composite_map.get(), pose);
                    std::cout <<"Upload Map Successfully1" << std::endl;
                    // auto maps = ((CompositeMap)*composite_map.get()).maps();
                    // for_each(maps.begin(), maps.end(), [](boost::shared_ptr<MapLayer> _layer){
                    //     boost::shared_ptr<MapLayer> layer = _layer;
                    //     std::cout << "name = " << layer->getName() << std::endl;
                    //     std::cout << "type = " << layer->getType() << std::endl;
                    //     std::cout << "usage = " << layer->getUsage() << std::endl;
                        
                    // });
                    std_msgs::String result;
                    result.data = "连接成功:";// + (ip_addr);
                    tts_pub.publish(result);
                    return;
                }
                composite_map = composite_map_reader.loadFile(errMsg, "map.txt.backup");
                
                if (errMsg.empty()) {
                    rpos::core::Pose pose;
                    loadPose(pose);
                    std::cout << pose.x() << "-" << pose.y() << "-" << pose.yaw() << std::endl;
                    sdp.setCompositeMap((CompositeMap)*composite_map.get(), pose);
                    std::cout <<"Upload Map Successfully2" << std::endl;                    
                    std_msgs::String result;
                    result.data = "连接成功:";// + (ip_addr);
                    tts_pub.publish(result);
                    return;
                }
            }
            // std::cout <<"SDK Version: " << sdp.getSDKVersion() << std::endl;
            // std::cout <<"SDP Version: " << sdp.getSDPVersion() << std::endl;
            // std::cout <<"Bartter Status: " << sdp.getBatteryIsCharging() << std::endl;
            // std::cout <<"Barttery Percetage: " << sdp.getBatteryPercentage() << std::endl;
            // std::cout <<"Power Status: " << sdp.getDCIsConnected() << std::endl;
            // sdp.setMapLocalization(true);
            // sdp.setMapUpdate(true);
        } catch(ConnectionTimeOutException& e) {
            std::cout <<e.what() << std::endl;
        } catch(ConnectionFailException& e) {
            std::cout <<e.what() << std::endl;
            // return 1;
            connected = false;
        }

    }
}

void actionCallback(const ros::TimerEvent&)
{
    if (!connected) return;

    if (g_dc_is_connected) {
        ROS_INFO("actionCallback g_dc_is_connected");
        return;
    }

    try {
        auto moveAction = sdp.getCurrentAction();
        std::cout << "moveAction:" << std::endl;
        if (moveAction) {
            auto actionStatus = moveAction.getStatus(); 
            std::cout << "actionStatus::::=" << actionStatus << std::endl;
            if (actionStatus == ActionStatus::ActionStatusFinished) {
                std_msgs::String result;
                result.data = "到达目的地";
                tts_pub.publish(result);

                ros::Duration(2).sleep();
                rpos::core::Pose home = sdp.getHomePose();
                rpos::core::Pose pose = sdp.getPose();
                if (!isNear(home, pose)) {
                    result.data = "我要回去了";
                    tts_pub.publish(result);
                    sdp.moveTo(home.location(), false, true);
                }
            }
            if (actionStatus == ActionStatus::ActionStatusError) {
                std_msgs::String result;
                result.data = "无法到达目的地";
                tts_pub.publish(result);
            }
        }
        /*
        todo :slow,get close
        */
    } catch(ConnectionFailException e) {
        std::cout << "moveAction:" << e.what() << std::endl;                
        connected = false;
    } catch(RequestTimeOutException& e) {
        std::cout << "moveAction:" << e.what() << std::endl;
    } catch(InvalidArgumentsException& e) {
        std::cout << "moveAction:" << e.what() << std::endl;
    }
}

void recoverCallback(const std_msgs::Empty::ConstPtr& e)
{
    if (connected) {
        try {
            // rpos::message::depth_camera::DepthCameraFrame depth_camera_frame;
            // sdp.publishDepthCamFrame(0, depth_camera_frame);
            // int quality = sdp.getLocalizationQuality();
            // if (quality <= LOCALIZATIONQUALITY) {
            rpos::core::RectangleF area;
            auto ma = sdp.recoverLocalization(area);
            ma.waitUntilDone();
            // }
        } catch(ConnectionFailException e) {
            std::cout << "recoverLocalization:" << e.what() << std::endl;                
            connected = false;
        } catch(RequestTimeOutException& e) {
            std::cout << "recoverLocalization:" << e.what() << std::endl;
        } catch(InvalidArgumentsException& e) {
            std::cout << "recoverLocalization:" << e.what() << std::endl;
        } catch(OperationFailException& e) {
            std::cout << "recoverCallback OperationFailException::" << e.what() << std::endl;
        }
    }
}

void setHomePoseCb(const manager::Pose::ConstPtr& msg)
{
    if (!connected) return;

    rpos::core::Location loc(msg->x, msg->y);
    rpos::core::Rotation rot(msg->yaw);
    rpos::core::Pose p(loc, rot);
    sdp.setHomePose(p);
}

void goHomeCallback(const ros::TimerEvent&)
{
    if (!connected) return;
    // if (!home_switch) return;
    if (g_dc_is_connected) {
        ROS_INFO("goHomeCallback g_dc_is_connected");
        return;
    }
    try {
        rpos::actions::MoveAction moveAction = sdp.getCurrentAction();
        if (!moveAction) {
            rpos::core::Pose home = sdp.getHomePose();
            rpos::core::Pose pose = sdp.getPose();
            if (!isNear(home, pose)) {
                if (action_complete) {
                    if (difftime(time(NULL), action_complete_time) > 10.0) {
                        auto ma = sdp.moveTo(home.location(), false, true);
                        std::cout << "goHome:" << std::endl;
                        std_msgs::String result;
                        result.data = "我要回去了";
                        tts_pub.publish(result);
                        action_complete = false;
                    }
                }
            }
        }

        if (percentage <= 30 && !g_dc_is_connected) {
            std_msgs::String result;
            result.data = "电量剩余" + std::to_string(percentage) + "%, 请充电";
            tts_pub.publish(result);
            rpos::core::Pose home = sdp.getHomePose();
            rpos::core::Pose pose = sdp.getPose();
            if (!isNear(home, pose)) {
                rpos::actions::MoveAction moveAction = sdp.getCurrentAction(); 
                if (!moveAction) {
                    // sdp.goHome();
                    auto ma = sdp.moveTo(home.location(), false, true);
                    std::cout << "goHome:" << std::endl;                
                }
            }
        }
    } catch(ConnectionFailException e) {
        std::cout << "goHomeCallback:" << e.what() << std::endl;                
        connected = false;
    } catch(RequestTimeOutException& e) {
        std::cout << "goHomeCallback:" << e.what() << std::endl;
    } catch(InvalidArgumentsException& e) {
        std::cout << "goHomeCallback:" << e.what() << std::endl;
    } catch(OperationFailException& e) {
        std::cout << "goHomeCallback " << e.what() << std::endl;
    }
}

bool homeSwitchCb(manager::Switch::Request& request, manager::Switch::Response& response)
{
    std_msgs::String result;

    home_switch = request.setSwitch;
    // if (home_switch)
    //     result.data = "关闭自动回充";
    // else
    //     result.data = "打开自动回充";
    // tts_pub.publish(result);

    response.status = home_switch;
    // home_switch = !home_switch;

    // response.re

    return true;
}

int main(int argc, char * argv[])
{
    ros::init(argc, argv, "manager");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("guide", 1000, cmdCallback);
    // ros::Subscriber ip_sub = n.subscribe("local_ip", 10, ipCallback);
    ros::Subscriber recover_sub = n.subscribe("recover_localization", 10, recoverCallback);
    ros::Subscriber set_pose_sub = n.subscribe("set_home_pose", 1, setHomePoseCb);
    ros::ServiceServer service = n.advertiseService("gohomeswitch", homeSwitchCb);
    
    tts_pub = n.advertise<std_msgs::String>("tts", 10);
    move_action_pub = n.advertise<std_msgs::String>("move_action", 10);
    battery_pub = n.advertise<manager::Battery>("battery", 1);

    ros::Timer timer = n.createTimer(ros::Duration(0.5), timerSaveCallback);
    // ros::Timer timer_heartbeat = n.createTimer(ros::Duration(heartbeatInterval - 1), timerHeartBeatCallback);
    ros::Timer connect_heartbeat = n.createTimer(ros::Duration(10), connectCallback);
    // ros::Timer action_heartbeat = n.createTimer(ros::Duration(10), actionCallback);
    ros::Timer home_heartbeat = n.createTimer(ros::Duration(10), goHomeCallback);

    ros::spin();
    // ros::Rate loop_rate(1);
    // while(ros::ok())
    // {
    //     ros::spinOnce(); 
    //     loop_rate.sleep();
    // }

    return 0;
}

